#include <math.h>
#include <opencv/cv.h>
#include <opencv/highgui.h>
#include <stdio.h>
#include <stdlib.h>
#include <iostream>
#include "ColorHSV.h"
#include "NamedColorRangeHSV.h"
#include "Engines.h"
#include "Sight.h"
#include "Smell.h"

#include <SerialStream.h>

using namespace std;
using namespace cv;
using namespace LibSerial;

#define ESC 27
#define WKEY 20
#define POWER 22
#define FULL 30

//#define DISTANCE 38
#define DISTANCE 20
#define BIG_DISTANCE 50
#define VELICINA 6

int main(int argc, char *argv[])
{

	Engines motor(8,FULL);
//	Engines motor(0, 0);
	Sight vid;
//	usleep(10000000);
	Smell smell;
	char taster;
	int X=0, Y=0;
//	motor.left(32);
//	motor.right(32);
//	usleep(20000000);
//	motor.right(0);
//	motor.left(0);
	while(1)
	{
//		smell.prepere(taster);
		vid.prepere(taster);
//		vid.roadObstacle(X, Y, VELICINA, 3);
		motor.left(FULL);
		motor.right(FULL);
		taster=waitKey(WKEY);
		if(taster==ESC) {motor.left(0);motor.right(0);return 1;}
//		while(Y<BIG_DISTANCE && Y!=0)
//		{
//			vid.prepere(taster);
//			vid.roadObstacle(X, Y, VELICINA, 3);
//			motor.left(POWER);
//			motor.right(POWER);
//			taster=waitKey(WKEY);
//			if(taster==ESC) {motor.left(0);motor.right(0);return 1;}
//			if(X<0)
//			{
//				while(Y<DISTANCE && Y!=0)
//				{
//					(Y>1)? motor.left(POWER):motor.left(0);
//					motor.right(-POWER);
//					vid.prepere(taster);
//					vid.roadObstacle(X, Y, VELICINA, 3);
//					taster=cvWaitKey(WKEY);
//					if(taster==ESC) {motor.left(0);motor.right(0);return 1;}
//				}
//			}
//			else
//			{
//				while(Y<DISTANCE && Y!=0)
//				{
//					motor.left(-POWER);
//					(Y>1)? motor.right(POWER):motor.right(0);
//					vid.prepere(taster);
//					vid.roadObstacle(X, Y, VELICINA, 3);
//					taster=cvWaitKey(WKEY);
//					if(taster==ESC) {motor.left(0);motor.right(0);return 1;}
//				}
//			}
//		}

////               daljinska kontrola
//
//		taster=waitKey(WKEY);
//		if(taster==ESC) {motor.left(0);motor.right(0);break;}
//		switch(taster)
//		{
//			case('w'): motor.left(POWER); motor.right(POWER); cout<<"na pred"<<endl; break;
//			case('s'): motor.left(-POWER); motor.right(-POWER); cout<<"nazad"<<endl; break;
//			case('d'): motor.left(POWER); motor.right(-POWER); cout<<"levo"<<endl; break;
//			case('a'): motor.left(-POWER); motor.right(POWER); cout<<"desno"<<endl;break;
//			case(' '): motor.left(0); motor.right(0); cout<<"STOP"<<endl;break;
//			//default: motor.left(0); motor.right(0); cout<<"STOP"<<endl;break;
//		}
//
//
//
//		vid.changeColors(taster);
//		vid.prepere(taster);

	}
}


